#ifndef ALG_ROBUST_HPP
#define ALG_ROBUST_HPP

#include <stack>

using namespace std;

class Robust: public PathAlg
{
public:
    Robust( void )
    {
        _status = LINE;
    }

    void init( Map &map )
    {
        int distanceX, distanceY;
        distanceX = map.dX() - map.sX();
        distanceY = map.dY() - map.sY();

        if( abs( distanceX ) > abs( distanceY ) )
        {
            double proportionY = abs( double( distanceY ) / double( distanceX ) ) * signal( distanceY );
            int proportionX = signal( distanceX );
            int limit = abs( distanceX );
            for( int i = 0; i <= limit; ++i )
            {
                map( i * proportionY + map.sY(), round( i * proportionX ) + map.sX() ) |= MARKED;
            }
            _dir1 = ( distanceX > 0 ) ? EAST : WEST;
            _dir2 = ( distanceY > 0 ) ? SOUTH : NORTH;
        }
        else if( distanceY == 0 )
        {
            map( map.sY(), map.sX() ) |= MARKED;
        }
        else // distanceY >= distanceY AND distanceY != 0
        {
            double proportionX = abs( double( distanceX ) / double( distanceY ) ) * signal( distanceX );
            int proportionY = signal( distanceY );
            int limit = abs( distanceY );
            for( int j = 0; j <= limit; ++j )
            {
                map( round( j * proportionY ) + map.sY(), j * proportionX + map.sX() ) |= MARKED;
            }
            _dir1 = ( distanceY > 0 ) ? SOUTH : NORTH;
            _dir2 = ( distanceX > 0 ) ? EAST : WEST;
        }
        map( map.dY(), map.dX() ) |= MARKED;
    }

    Result step( Map &map )
    {
        if( map.x() == map.dX() && map.y() == map.dY() ) return ALG_SUCCEEDED;
        if( _status == LINE )
        {
            Uint8 dir2walk;
            if( map.hasFlag( MARKED, _dir1 ) )
                dir2walk = _dir1;
            else if( map.hasFlag( MARKED, _dir2 ) )
                dir2walk = _dir2;
            else if( map.dir() == _dir2 )
                dir2walk = _dir1;
            else dir2walk = _dir2;
            if( map.isBlock( dir2walk ) )
            {
                // Obstáculo perpendicular
                if(map.dir() == dir2walk) // Same direction
                {
                    if( !map.isBlock( turnClock(dir2walk) ))
                    {
                        map.walk(turnClock(dir2walk) );
                        _clockwise = false;

                    }
                    else if( !map.isBlock( turnAntiClock(dir2walk) ))
                    {
                        map.walk(turnAntiClock(dir2walk) );
                        _clockwise = true;

                    }
                    else
                    {//Change it!
                        map.walk(reverseDir(dir2walk) );
                        _clockwise = false;
                    }

                }
                //Obstaculo paralelo
                else // Change planned
                {
                    _clockwise = isClock(map.dir(), dir2walk);
                    if( !map.isBlock(map.dir()))
                        map.walk(map.dir());
                    else if( !map.isBlock(turn(map.dir(), !_clockwise)) )
                        map.walk(turn(map.dir(), !_clockwise));
                    else
                        map.walk(reverseDir(dir2walk) );
                }
                _status = ROUNDING;
            }
            else
                map.walk( dir2walk );
        }
        else if(_status == ROUNDING)
        {
            Uint8 dir = turn(map.dir(), _clockwise);
            while( map.isBlock(dir)) // Look for a fre direction
                dir = turn(dir, !_clockwise);
            if( map.hasFlag(MARKED, dir)
               && !map.hasFlag(VISITED,reverseDir(map.dir()))
               && !map.hasFlag(LK_NORTH | LK_SOUTH | LK_EAST | LK_WEST, dir) )
                _status = LINE;
            map.walk(dir);
        }
        return ALG_PROCESSING;
    }
private:
    enum Status
    {
        LINE,
        ROUNDING
    };

    bool _clockwise;

    Status _status;
    Uint8 _dir1;
    Uint8 _dir2;
};


#endif // ALG_ROBUST_HPP

